/**
 * @file
 * @author dousha 
 * @date 2021/4/22.
 */

#include "solver.h"
#include "mpu.h"
#include "gps.h"
#include "system.h"
#include "qfplib-m3.h"
#include "quaternionFilter.h"
#include "util.h"
#include "display.h"
#include "blinker.h"

#define USE_LIBRARY 0
#define IGNORE_MAG 0
#define USE_GIVEN_YAW 1

#define YAW_BIAS (HALF_PI)

#define CONSIDER_SMALL_ANGLE_ONLY 1

static Position selfPosition, targetPosition, estimatedPosition;
static MpuData *mpuRaw = NULL, mpuCorrected;
static MpuCalibrationData *mpuCalib = NULL;
static GpsData *gps = NULL;
static Direction solution, expectation = { .pitch = 450, .yaw = 900 };
static uint32_t lastSolverTick = 0, lastGpsFrame = 0, lastGpsTick = 0;

const static float RAD_TO_DEG = 180.f / 3.14159f;
const static float M_RES = 0.00000015f;
const static float HALF_PI = 3.14159f / 2.f;

extern DisplayInfo *info;

float to_degree(float x) {
	return qfp_fmul(x, RAD_TO_DEG);
}

int16_t to_fixed(float x) {
	return (int16_t) (x * 10.f);
}

void solver_init() {
	mpuRaw = (MpuData *) mpu_get();
	mpuCalib = (MpuCalibrationData *) mpu_calibration_get();
	gps = (GpsData *) gps_get_last_frame();

	selfPosition.elevation = 0;
	selfPosition.latitude = 0;
	selfPosition.longitude = 0;
	selfPosition.pitch = 0.f;
	selfPosition.yaw = 0.f;
	selfPosition.roll = 0.f;

	targetPosition.elevation = 0;
	targetPosition.latitude = 0;
	targetPosition.longitude = 0;
	targetPosition.pitch = 0.f;
	targetPosition.yaw = 0.f;
	targetPosition.roll = 0.f;
}

void solver_tick() {
	uint32_t now = HAL_GetTick();
	if (system_is_module_ready(MODULE_GPS)) {
		selfPosition.elevation = gps->elevation;
		selfPosition.latitude = gps->lat;
		selfPosition.longitude = gps->lng;
		if (lastGpsFrame != gps->frame) {
			lastGpsFrame = gps->frame;
			lastGpsTick = now;
		}
		if (now - lastGpsTick > 2000) {
			blinker_off();
		} else {
			blinker_on();
		}
	}
	if (system_is_module_ready(MODULE_GYROSCOPE)) {
		for (uint8_t i = 0; i < 3; i++) {
			mpuCorrected.acceleration[i] = mpuRaw->acceleration[i];
			mpuCorrected.gyro[i] = mpuRaw->gyro[i]; // gyro is pre-corrected;
			mpuCorrected.magnetic[i] = mpuRaw->magnetic[i] - mpuCalib->magneticBias[i];
		}
#if USE_LIBRARY
		update_estimate(mpuRaw, &selfPosition);
#else
		float normalizedX = qfp_fdiv(mpuCorrected.acceleration[X_AXIS], 8192.f);
		float normalizedY = qfp_fdiv(mpuCorrected.acceleration[Y_AXIS], 8192.f);
		float normalizedZ = qfp_fdiv(mpuCorrected.acceleration[Z_AXIS], 8192.f);
		selfPosition.roll = qfp_fatan2(normalizedX, normalizedZ);
		selfPosition.pitch = qfp_fatan2(normalizedY, normalizedZ);
		update_estimate(&mpuCorrected, &estimatedPosition);
#if USE_GIVEN_YAW
		selfPosition.yaw = estimatedPosition.yaw;
		selfPosition.yaw += YAW_BIAS;
		selfPosition.yaw = (4 * HALF_PI) - selfPosition.yaw - mpuCalib->magneticInclination;
		if (selfPosition.yaw > 4 * HALF_PI) {
			selfPosition.yaw -= 4 * HALF_PI;
		}
		if (selfPosition.yaw < 0) {
			selfPosition.yaw += 4 * HALF_PI;
		}
		//selfPosition.yaw = qfp_fatan2(normalizedMy, normalizedMx);
#else
#if IGNORE_MAG
		// always assume starting angle is 0
		if (lastSolverTick > 0) {
			float deltaT = (now - lastSolverTick) / 1000.f;
			float yawDiff = deltaT * mpuRaw->gyro[Z_AXIS];
			selfPosition.yaw += yawDiff;
		}
		lastSolverTick = now;
#else
		float scaledMx = qfp_fmul(mpuRaw->magnetic[0], mpuCalib->magneticScaling[0]);
		float scaledMy = qfp_fmul(mpuRaw->magneticScaling[1], mpuCalib->magneticScaling[1]);
		float scaledMz = qfp_fmul(mpuRaw->magneticScaling[2], mpuCalib->magneticScaling[2]);
#if HAS_MAG_CALIBRATION
		float normalizedMx = map(mpuRaw->magneticScaling[0], mpuCalib->magneticLowerBound[0], mpuCalib->magneticUpperBound[0]);
		float normalizedMy = map(mpuRaw->magneticScaling[1], mpuCalib->magneticLowerBound[1], mpuCalib->magneticUpperBound[1]);
		float normalizedMz = map(mpuRaw->magneticScaling[2], mpuCalib->magneticLowerBound[2], mpuCalib->magneticUpperBound[2]);
#else
		float norm = qfp_fsqrt(qfp_fmul(scaledMx, scaledMx) + qfp_fmul(scaledMy, scaledMy) + qfp_fmul(scaledMz, scaledMz));
		float normalizedMx = qfp_fdiv(scaledMx, norm);
		float normalizedMy = qfp_fdiv(scaledMy, norm);
		float normalizedMz = qfp_fdiv(scaledMz, norm);
#endif


#if CONSIDER_SMALL_ANGLE_ONLY
		selfPosition.yaw = qfp_fatan2(normalizedMy, normalizedMx);
#else
		float cosPitch = qfp_fcos(selfPosition.pitch);
		float sinPitch = qfp_fsin(selfPosition.pitch);
		float cosRoll = qfp_fcos(selfPosition.roll);
		float sinRoll = qfp_fsin(selfPosition.roll);
		float magX = qfp_fmul(normalizedMx, qfp_fcos(selfPosition.pitch));
		magX = qfp_fadd(magX, qfp_fmul(qfp_fmul(normalizedMy, sinRoll), sinPitch));
		magX = qfp_fsub(magX, qfp_fmul(qfp_fmul(normalizedMz, cosRoll), sinPitch));
		float magY = qfp_fmul(normalizedMy, cosRoll);
		magY = qfp_fadd(magY, qfp_fmul(normalizedMz, sinRoll));
		selfPosition.yaw = qfp_fatan2(magY, magX);
#endif
#endif
#endif
#endif
	}
}

const Position *solver_get() {
	return &selfPosition;
}

void solver_set_current_as_remote() {
	info->remoteLat = targetPosition.latitude = selfPosition.latitude;
	info->remoteLng = targetPosition.longitude = selfPosition.longitude;
	info->remoteAlt = targetPosition.elevation = selfPosition.elevation;
}

const Direction *solver_solve() {
	int16_t pitch = (int16_t) expectation.pitch;
	int16_t servoZeroingYaw = to_fixed(to_degree(selfPosition.yaw)) + 900;
	int16_t yaw = servoZeroingYaw - expectation.yaw;

	pitch -= to_fixed(to_degree(selfPosition.pitch)) * qfp_fsin(deg2rad((float) yaw / 10.f));
	pitch -= to_fixed(to_degree(selfPosition.roll)) * qfp_fcos(deg2rad((float) yaw) / 10.f);

	if (pitch < 0) {
		pitch = 0;
	}
	if (pitch > 1800) {
		pitch = 1800;
	}

	while (yaw < 0) {
		yaw += 3600;
	}
	while (yaw > 3600) {
		yaw -= 3600;
	}

	if (yaw > 1800) {
		pitch = 1800 - pitch;
	}

	solution.pitch = pitch;
	solution.yaw = yaw;

	return &solution;
}

void solver_set_target_pitch(uint16_t pitch) {
	expectation.pitch = pitch;
	info->expectedPitch = (int16_t) pitch;
}

void solver_set_target_yaw(uint16_t yaw) {
	expectation.yaw = yaw;
	info->expectedYaw = (int16_t) yaw;
}

void solver_set_self_yaw(uint16_t yaw) {
	selfPosition.yaw = deg2rad(((float) yaw) / 10.f);
}

int16_t solver_get_self_pitch() {
	return to_fixed(to_degree(selfPosition.pitch));
}

int16_t solver_get_self_yaw() {
	return to_fixed(to_degree(selfPosition.yaw));
}

void solver_set_remote_lat(int32_t lat) {
	targetPosition.latitude = lat;
	info->remoteLat = lat;
}

void solver_set_remote_lng(int32_t lng) {
	targetPosition.longitude = lng;
	info->remoteLng = lng;
}

void solver_set_remote_alt(int32_t alt) {
	targetPosition.elevation = selfPosition.elevation + alt;
	info->remoteAlt = targetPosition.elevation;
}

const MpuData *solver_get_corrected_mpu_data() {
	return &mpuCorrected;
}

void solver_mark_current_yaw_as_zero() {
	mpuCalib->magneticInclination = selfPosition.yaw;
}
